# 这是一个示例 Python 脚本。
import numpy as np
import cv2
from matplotlib import pyplot as plt

# 按 Shift+F10 执行或将其替换为您的代码。
# 按 双击 Shift 在所有地方搜索类、文件、工具窗口、操作和设置。
from eight_point_alg import eight_point, FindFundamentalMatrix
from essential_matrix import essential_matrix
from triangulation import get_camera_matrices, triangulate, project_back
from assignment2.python.helper import camera2
from epipolar_correspondences import epipolar_correspondences
from assignment2.python.helper import displayEpipolarF, epipolarMatchGUI


def print_hi(name):
    # 在下面的代码行中使用断点来调试脚本。
    print(f'Hi, {name}')  # 按 Ctrl+F8 切换断点。


# 按间距中的绿色按钮以运行脚本。
if __name__ == '__main__':
    print_hi('PyCharm')
    # how = np.load('data/extrinsics.npz')
    # print(how.files)
    # print("???")
    test = np.load('assignment2/data/some_corresp.npz')
    # print(test.files)
    img1 = cv2.imread('assignment2/data/im1.png')
    img2 = cv2.imread('assignment2/data/im2.png')
    # M = img1.shape[0:2]
    # print(M)
    pt1, pt2 = test['pts1'], test['pts2']
    # F = eight_point(pt1, pt2, M)
    F = FindFundamentalMatrix(pt1, pt2)

    # part1
    # displayEpipolarF(img1, img2, F)

    # part2
    # epipolarMatchGUI(img1, img2, F)

    # part3
    k_load = np.load('assignment2/data/intrinsics.npz')
    k1, k2 = k_load['K1'], k_load['K2']
    E = essential_matrix(F, k1, k2)
    # print(E)

    # part4&final
    p_load = np.load('assignment2/data/temple_coords.npz')
    # print(p_load)
    pt1_c = p_load['pts1']
    pts2_compute = epipolar_correspondences(img1, img2, F, pt1_c)

    M2s = camera2(E)

    # print(M2s)
    for i in range(0, 4):
        P2 = M2s[:, :, i]
        cam_mat1, cam_mat2 = get_camera_matrices(P2, k1, k2)
        # print(pts2_compute.shape)
        pts3 = triangulate(cam_mat1, pt1_c, cam_mat2, pts2_compute)
        project_back(pts3, cam_mat1, pt1_c)
        # print(pt1_c.shape)
        ax = plt.subplot(projection='3d')  # 创建一个三维的绘图工程
        ax.set_title('3d_image_show')  # 设置本图名称
        ax.scatter(pts3[:, 0], pts3[:, 1], pts3[:, 2], c='b')  # 绘制数据点 c: 'r'红色，'y'黄色，等颜色

        ax.set_xlabel('X')  # 设置x坐标轴
        ax.set_ylabel('Y')  # 设置y坐标轴
        ax.set_zlabel('Z')  # 设置z坐标轴
        ax.set_zlim(4, 6)
        ax.set_xlim(-1, 1)
        ax.set_ylim(-1, 1)
        plt.show()

        # P2 = M2s[:, :, i]
        # cam_mat1, cam_mat2 = get_camera_matrices(P2, k1, k2)
        # #print(pts2_compute.shape)
        # pts3 = triangulate(cam_mat1, pt1, cam_mat2, pt2)
        # project_back(pts3, cam_mat1, pt1)
        # #print(pt1_c.shape)
        # ax = plt.subplot(projection='3d')  # 创建一个三维的绘图工程
        # ax.set_title('3d_image_show')  # 设置本图名称
        # ax.scatter(pts3[:, 0], pts3[:, 1], pts3[:, 2], c='b')  # 绘制数据点 c: 'r'红色，'y'黄色，等颜色
        #
        # ax.set_xlabel('X')  # 设置x坐标轴
        # ax.set_ylabel('Y')  # 设置y坐标轴
        # ax.set_zlabel('Z')  # 设置z坐标轴
        # ax.set_zlim(4, 6)
        # ax.set_xlim(-1, 1)
        # ax.set_ylim(-1, 1)
        # plt.show()

    R1 = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
    t1 = np.array([[0], [0], [0]])
    R2 = M2s[:, :, 1][:, 0:3]
    t2 = M2s[:, :, 1][:, 3]
    # print(M2s[:,:,1])
    # print(R2)
    # print(t2)
    # print(R1)
    # print(t1)
    # data_1 = (R1, R2, t1, t2)
    np.savez("extrinsics.npz", R1, R2, t1, t2)

# 访问 https://www.jetbrains.com/help/pycharm/ 获取 PyCharm 帮助
